#include "vtor_pid.h"

#ifdef __VTOR_PID__

void VtorPid_Clear(VtorPid* pid)
{
	pid->totalErr = 0;
	pid->outi = 0;
}

void VtorPid_Calc(VtorPid* pid, float intervalTime)
{
	pid->intervalTime = intervalTime;
	pid->err = pid->exp - pid->cur;
	pid->totalErr += pid->err;
	if(pid->totalErrMax > pid->totalErrMin)
	{
		pid->totalErr > pid->totalErrMax
			? pid->totalErr = pid->totalErrMax : 0;
		pid->totalErr < pid->totalErrMin
			? pid->totalErr = pid->totalErrMin : 0;
	}
	
	pid->outp = pid->kp * pid->err * pid->intervalTime;
	pid->outi = pid->ki * pid->totalErr * pid->intervalTime;
	pid->outd = pid->kd * (pid->err - pid->lastErr) / pid->intervalTime;

	pid->out = pid->outp + pid->outi + pid->outd;
	if(pid->outMax > pid->outMin)
	{
		pid->out = pid->out > pid->outMax ? pid->outMax : pid->out;
		pid->out = pid->out < pid->outMin ? pid->outMin : pid->out;
	}
	
	pid->lastErr = pid->err;
}

void VtorPid_SetExp(VtorPid* pid, float exp)
{
	pid->exp = exp;
}

void VtorPid_SetCur(VtorPid* pid, float cur)
{
	pid->cur = cur;
}

#endif // __VTOR_PID__

